%sample code, patch test for T3 element with separate container for element
%variables
function PatchTestT3_3
clc
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% mesh
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nodes = [ 0 0;
    1 0;
    1 1;
    0 1 ];
connectivities = [ 1 2 3;
    1 3 4];
mesh = createMesh(nodes,connectivities);
%mesh.printMyself();

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% conditions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
du = 0.1;
constraint = [  1 0 0 0.0 0.0;
    2 0 0 du 0.0;
    3 0 1 du 0.0;
    4 0 1 0.0 0.0   ];
setConstraints(mesh,constraint);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% element declaration; custom section
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
M = fem2.Model(mesh);
nf = mesh.countFaces();
E = 2;
nu = 0.3;
C = E/(1-nu*nu)*...
    [   1 nu 0;
    nu 1 0;
    0 0 (1-nu)/2    ];
for i=1:nf
    e = fem2.matlab.T3MatlabElement(mesh.getFace(i-1));
    M.addElement(e);
    %create struct to contain element state variables
    es = struct('stress',[0 0 0],'strain',[0 0 0],'constitutiveMatrix',C);
    elements(i) = es;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% time steps iteration
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
numTimeStep = 10;
M.preAnalysis();
nDofs = mesh.countDofs();
r = zeros(nDofs,1);
M.assembleR(r);
u = zeros(nDofs,1);
tol = 1e-6;
for i=1:numTimeStep
    lambda = i/numTimeStep;
    lr = lambda*r;
    %update model's states
    M.setX(lambda,u);
    elements = updateStates(elements,M);
    
    %assemble ri
    ri = assembleRi(elements,M);
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Newton-Raphson loop
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%
    cnt = 0;
    while 1
        %assemble kt
        kt = assembleKt(elements,M);
        
        du = kt\(lr-ri);
        u = u + du;
        M.setX(lambda,u);
        
        %update model's states
        elements = updateStates(elements,M);
        
        %assemble ri
        ri = assembleRi(elements,M);
        
        %compute convergent criteria
        res = norm(r-ri);
        cnt = cnt+1;
        if res < tol
            disp(['Newton Raphson iteration converged in ' num2str(cnt) ' loops']);
            break;
        end
    end
    disp(['time step ' num2str(i) ' converged']);
end


end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% assembly routines; fixed section
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function ri = assembleRi(elements,model)
mesh = model.getMesh();
nf = mesh.countFaces();
nDofs = mesh.countDofs();
ri = zeros(nDofs,1);
for i=1:nf
    rie = computeRie(i,elements,model);
    dofs = mesh.getFace(i-1).getDofNumbers();
    len = size(dofs(:),1);
    for j=1:len
        if dofs(j)~=-1
            ri(dofs(j)+1) = ri(dofs(j)+1) + rie(j);
        end
    end
end
end

function kt = assembleKt(elements,model)
mesh = model.getMesh();
nf = mesh.countFaces();
nDofs = mesh.countDofs();
kt = zeros(nDofs,nDofs);
for i=1:nf
    kte = computeKte(i,elements,model);
    dofs = mesh.getFace(i-1).getDofNumbers();
    len = size(dofs(:),1);
    for j=1:len
        for k=1:len
            if dofs(j)~=-1 && dofs(k)~=-1
                kt(dofs(j)+1,dofs(k)+1) = kt(dofs(j)+1,dofs(k)+1) + kte(j,k);
            end
        end
    end
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% element routines; custom section
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function kte = computeKte(i,elements,model)
e = model.getElement(i-1);
ng = e.getNumGaussPoint();
kte = zeros(6,6);
for j=1:ng
    xi = e.getGaussPoint(j-1);
    w = e.getGaussWeight(j-1);
    jac = e.getFace().jacXAt(xi(1),xi(2));
    B = zeros(3,6);
    B = e.computeB(xi(1),xi(2),e.getFace(),B);
    C = elements(i).constitutiveMatrix(:,:,j);
    kte = kte+w*B'*C*B*abs(det(jac));
end
end

function rie = computeRie(i,elements,model)
e = model.getElement(i-1);
ng = e.getNumGaussPoint();
rie = zeros(6,1);
for j=1:ng
    xi = e.getGaussPoint(j-1);
    w = e.getGaussWeight(j-1);
    jac = e.getFace().jacXAt(xi(1),xi(2));
    B = zeros(3,6);
    B = e.computeB(xi(1),xi(2),e.getMeshPart(),B);
    sigma = elements(i).stress(j,:)';
    rie = rie+w*B'*sigma*abs(det(jac));
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% material law routines; custom section
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function elements = updateStates(elements,model)
ne = model.countElements();
%if yes, update the constitutive matrix first
for i=1:ne
    e = model.getElement(i-1);
    f = e.getFace();
    ue = f.getUHatLocal();
    ng = e.getNumGaussPoint();
    for j=1:ng
        xi = e.getGaussPoint(j-1);
        B = zeros(3,6);
        B = e.computeB(xi(1),xi(2),e.getMeshPart(),B);
        C = elements(i).constitutiveMatrix(:,:,j);
        epsilon = B*ue;
        sigma = C*epsilon;
        elements(i).stress(j,:) = sigma;
        elements(i).strain(j,:) = epsilon;
    end
end
end
